
# def constraintsAdjacent(s1,i,s2):
#     x_s1 = sum(pos_grid[start_s1+j:start_s1+j+15,0])/15
#     y_s1 = sum(pos_grid[start_s1+j:start_s1+j+15,1])/15
#     x_s2 = sum(pos_grid[start_s2:stop_s2+1,0])/15
#     y_s2 = sum(pos_grid[start_s2:stop_s2+1,1])/15
#     radius = math.sqrt((x_s1-x_s2)**2 + (y_s1-y_s2)**2)




from compute_f import wrapTo180
import numpy as np
from compute_f import *

def sequenceMatch(s1,s2):
    '''
    file    :loop_closure_f.py
    descri  :找当前磁序列窗口在历史序列中的最佳匹配段
    time    :2021/09/04 10:31:10
    author :binyao
    version :1.0
    input   :
            s1：历史序列
            s2：当前窗口磁序列
    output  :
            在历史序列中的匹配段的左下索引值
    '''
    left = 0
    right = len(s1) - 1
    s2_length = len(s2)
    match_index = left

    min_distance = sum(abs(s1[left:left+s2_length] - s2))
    left += 1
    while left + s2_length <= right - s2_length:
        tmp = sum(abs(s1[left:left+s2_length] - s2))
        if tmp < min_distance:
            min_distance = tmp
            match_index = left
        left += 1
    return match_index

def constraintsLength(s1_pos,s2_pos):
    '''
    file    :loop_closure_f.py
    descri  :匹配的两段轨迹长度应该一致
    time    :2021/12/07 18:47:19
    author :binyao
    version :1.0
    input   :
    output  :
    '''
    len_s1 = np.linalg.norm(s1_pos[0, 0:2] - s1_pos[-1, 0:2])
    len_s2 = np.linalg.norm(s2_pos[0, 0:2] - s2_pos[-1, 0:2])
    len_diff = abs(len_s2 - len_s1)
    
    # print('[radius]: %f'%(radius))
    return len_diff < 1

def constraintsAdjacent(s1_pos,s2_pos):

    x = (s1_pos[:,0] - s2_pos[:,0]).T*(s1_pos[:,0] - s2_pos[:,0])
    y = (s1_pos[:,1] - s2_pos[:,1]).T*(s1_pos[:,1] - s2_pos[:,1])
    radius = max(np.sqrt(x + y))
    # print(radius)
    print('[radius]        : %f'%(radius))
    return radius < 10

def calDistoLine(s1_pos):
    '''
    file    :loop_closure_f.py
    descri  :计算一条轨迹到由两个端点构成的直线的距离 的向量
    time    :2021/11/06 16:02:53
    author :binyao
    version :1.0
    input   :
    output  :
    '''
    # from scipy.stats import stats
    # import math

    absDisToLine1 = np.zeros(len(s1_pos[:,0]) - 2) #首尾距离为0，不用算
    x0 = s1_pos[0,0]
    x1 = s1_pos[-1,0]
    y0 = s1_pos[0,1]
    y1 = s1_pos[-1,1]
    
    # y= kx +b
    # d= |kx0-y0+b|/sqrt(k^2+1)
    k = (y1 - y0)/(x1 - x0)
    b = y0-k*x0

    frac = np.sqrt(k**2 + 1)
    for i in range(1,len(s1_pos[:,0])-1):
        x2 = s1_pos[i,0]
        y2 = s1_pos[i,1]
        d = abs(k*x2-y2+b)/frac
        absDisToLine1[i - 1] = d
    return absDisToLine1

def constraintsTopologySim(s1_pos,s2_pos):
    from scipy.stats import stats
    import math
    from numpy.lib.function_base import diff

    # euclideDis1 = np.sqrt(diff(s1_pos[:,0]).T*diff(s1_pos[:,0]) + diff(s1_pos[:,1]).T*diff(s1_pos[:,1]))
    # euclideDis2 = np.sqrt(diff(s2_pos[:,0]).T*diff(s2_pos[:,0]) + diff(s2_pos[:,1]).T*diff(s2_pos[:,1]))
    # r,p_value = stats.pearsonr(euclideDis1,euclideDis2)
    # print(r)
    # return True
    absDisToLine1 = calDistoLine(s1_pos)
    absDisToLine2 = calDistoLine(s2_pos)
    r,p_value = stats.pearsonr(absDisToLine1,absDisToLine2)
    print('[Topo pearson]  :%f'%(r))
    return abs(r) > 0.95

def constraintsMagSequenceSim(s1_pos,s2_pos):
    from scipy.stats import stats
    import math
    # pass
    # print(len(s1_pos[:, 7].any()))
    # print(len(s1_pos[:, 7].all()))
    r,p_value = stats.pearsonr(s1_pos[:, 7],s2_pos[:, 7])
    print('[MagSeq pearson]:%f'%(r))
    return abs(r) > 0.95


def constraint(s1_pos,s2_pos):
    '''
    file    :loop_closure_f.py
    descri  :回环验证是否满足约束条件
    time    :2021/12/07 18:57:34
    author :binyao
    version :1.0
    input   :
            s1_pos: # 0 X, 1 Y, 2 Z, 3 ori, 4 magx, 5 magy, 6 magz, 7 mag_xyz
            s2_pos: # 0 X, 1 Y, 2 Z, 3 ori, 4 magx, 5 magy, 6 magz, 7 mag_xyz
    output  :
            True or False: 回环验证是否满足约束条件
    '''
    res = True
    res = constraintsAdjacent(s1_pos,s2_pos) and constraintsLength(s1_pos,s2_pos) and constraintsTopologySim(s1_pos,s2_pos) and constraintsMagSequenceSim(s1_pos,s2_pos)
    # res = constraintsMagSequenceSim(s1_pos,s2_pos)

    # res = constraintsAdjacent(s1_pos,s2_pos) 
    return res

def initGraph(pos_grid,sim_loop):
    '''
    file    :loop_closure_f.py
    descri  :
    time    :2021/09/10 17:14:33
    author :binyao
    version :1.0
    input   :
            pos_grid # 0 X, 1 Y, 2 Z, 3 ori, 4 magx, 5 magy, 6 magz, 7 mag_xyz
    output  :
    '''
    import numpy as np
    import scipy.io as scio
    import math
    import matplotlib.pyplot as plt

    
    
      
    vertices = np.zeros((pos_grid.shape[0],4), dtype=float)
    for i in range(pos_grid.shape[0]):
        vertices[i,0] = i
        vertices[i,1:3] = pos_grid[i,0:2].copy()
        vertices[i,3] = math.radians(wrapTo180(pos_grid[i,3].copy()))
        # file_DK.write('VERTEX_SE2 %d %f %f %f'%())
        # vertices[i,3] = pos_grid[i,3].copy()
    # plt.figure(figsize=(15, 5))
    # plt.plot(vertices[4000:,3],marker=',')
    # plt.show()
    # weight = np.array([20.000000,0.000000,20.000000,1000.000,0.000000,0.000000]) #协方差信息矩阵，暂定这样的懒得算了
    weight = np.array([100, 0, 0, 100, 0, 1],dtype=np.int32)
    # weight = np.array([0.1, 0, 0, 0.1, 0, 0.1],dtype=np.float)
    edges = []
    for i in range(1,len(vertices)):
        
        theta1 = math.radians(wrapTo180(vertices[i-1,3]))
        theta2 = math.radians(wrapTo180(vertices[i,3]))
        v1 = vertices[i-1,1:4].copy()
        v2 = vertices[i,1:4].copy()
        v1[-1] = theta1
        v2[-1] = theta2
        A = np.dot(np.linalg.inv(v2t(v1)),v2t(v2))
        trans = t2v(A)
        # trans = v2 - v1
        if i == 1:
            print(trans)
        # idfrom_idto = np.array([vertices[i-1,0],vertices[i,0]])
        idfrom_idto = np.array([i-1,i])
        edges.append(np.hstack((idfrom_idto,trans,weight)))
    # list_valid = [1,3,4,5]
    # list_valid = [2]
    # sim_loop = [sim_loop[i] for i in list_valid]
    for loop_closure in sim_loop:
        trans = np.array([0,0,0])
        for i in range(len(loop_closure[0])):
            idfrom_idto = np.array([loop_closure[1][i],loop_closure[0][i]])
            edges.append(np.hstack((idfrom_idto,trans,weight)))
    edges = np.array(edges)
    # plt.figure(figsize=(15, 5))
    # plt.plot(vertices[4000:,3],marker=',')
    # plt.show()

    with open("DK.g2o","w") as file_DK:
        for each in vertices:
            file_DK.write('VERTEX_SE2 %d %f %f %f\n'%(each[0] + 1, each[1], each[2], each[3]))
        
        for loop_closure in sim_loop:
            trans = np.array([0,0,0],dtype=np.int32)
            file_DK.write('MiniBatchStart \n')
            for i in range(len(loop_closure[0])):
                idfrom_idto = np.array([loop_closure[1][i],loop_closure[0][i]],dtype=np.int32)
                each = (np.hstack((idfrom_idto,trans,weight)))
                file_DK.write('EDGE_SE2 %d %d %d %d %d %d %d %d %d %d %d\n'%(each[0]+ 1, each[1]+ 1, each[2], each[3], each[4],each[5], each[6],each[7], each[8], each[9], each[10]))
            file_DK.write('MiniBatchEnd \n')
            print(('EDGE_SE2 %d %d %d %d %d %d %d %d %d %d %d\n'%(each[0], each[1], each[2], each[3], each[4],each[5], each[6],each[7], each[8], each[9], each[10])))
            
    file_DK.close()
# MiniBatchEnd 
# # 
#     scio.savemat('car_e.mat', {'edge':edges})
#     scio.savemat('car_v.mat', {'vertices':vertices})

def initGraphAdjVertexs(pos_grid,sim_loop):
    '''
    file    :loop_closure_f.py
    descri  :
    time    :2021/09/10 17:14:33
    author :binyao
    version :1.0
    input   :
            pos_grid # 0 X, 1 Y, 2 Z, 3 ori, 4 magx, 5 magy, 6 magz, 7 mag_xyz
    output  :
    '''
    import numpy as np
    import scipy.io as scio
    import math
    import matplotlib.pyplot as plt

    vertices = np.zeros((pos_grid.shape[0],4), dtype=float) #顶点: 0 id, 1 x, 2 y, 3 heading 
    for i in range(pos_grid.shape[0]):
        vertices[i,0] = i
        vertices[i,1:3] = pos_grid[i,0:2].copy()
        vertices[i,3] = math.radians(wrapTo180(pos_grid[i,3].copy()))
    # weight = np.array([20.000000,0.000000,20.000000,1000.000,0.000000,0.000000]) #协方差信息矩阵，暂定这样的懒得算了
    weight = np.array([1, 0, 0, 1, 0, 1]*1000,dtype=np.float)
    
    with open("DK.g2o","w") as file_DK:
        for each in vertices:
            file_DK.write('VERTEX_SE2 %d %f %f %f\n'%(each[0] , each[1], each[2], each[3]))
        weight = np.array([1, 0, 0, 1, 0, 1],dtype=np.float)*100
        for i in range(1,len(vertices)):
            theta1 = vertices[i-1,3]
            theta2 = vertices[i,3]
            v1 = vertices[i-1,1:4].copy()
            v2 = vertices[i,1:4].copy()
            v1[-1] = theta1
            v2[-1] = theta2
            A = np.dot(np.linalg.inv(v2t(v1)),v2t(v2))
            trans = t2v(A)
            # trans = v2 - v1
            if i == 1:
                print(trans)
            # idfrom_idto = np.array([vertices[i-1,0],vertices[i,0]])
            idfrom_idto = np.array([i-1,i])
            each = (np.hstack((idfrom_idto,trans,weight)))
            file_DK.write('EDGE_SE2 %d %d %f %f %f %f %f %f %f %f %f\n'%(each[0], each[1], each[2], each[3], each[4],each[5], each[6],each[7], each[8], each[9], each[10]))
        weight = np.array([1, 0, 0, 1, 0, 1],dtype=np.float)*10
        for loop_closure in sim_loop:
            trans = np.array([0,0,0],dtype=np.int32)
            file_DK.write('MiniBatchStart \n')
            for i in range(len(loop_closure[0])):
                idfrom_idto = np.array([loop_closure[1][i],loop_closure[0][i]],dtype=np.int32)
                each = (np.hstack((idfrom_idto,trans,weight)))
                file_DK.write('EDGE_SE2 %d %d %d %d %d %d %d %d %d %d %d\n'%(each[0], each[1], each[2], each[3], each[4],each[5], each[6],each[7], each[8], each[9], each[10]))
            file_DK.write('MiniBatchEnd \n')
            print(('EDGE_SE2 %d %d %f %f %f %f %f %f %f %f %f\n'%(each[0], each[1], each[2], each[3], each[4],each[5], each[6],each[7], each[8], each[9], each[10])))
    file_DK.close()
    print('LoopClosure Write Success')
    # scio.savemat('car_e.mat', {'edge':edges})
    # scio.savemat('car_v.mat', {'vertices':vertices})

def initGraphRelative(pos_grid):
    '''
    file    :loop_closure_f.py
    descri  :
    time    :2021/09/10 17:14:33
    author :binyao
    version :1.0
    input   :
            pos_grid # 0 X, 1 Y, 2 Z, 3 ori, 4 magx, 5 magy, 6 magz, 7 mag_xyz
    output  :
    '''
    import numpy as np
    import scipy.io as scio
    import math
    import matplotlib.pyplot as plt

    vertices = np.zeros((pos_grid.shape[0],4), dtype=float) #顶点: 0 id, 1 x, 2 y, 3 heading 
    for i in range(pos_grid.shape[0]):
        vertices[i,0] = i
        vertices[i,1:3] = pos_grid[i,0:2].copy()
        vertices[i,3] = math.radians(wrapTo180(pos_grid[i,3].copy()))
    # weight = np.array([20.000000,0.000000,20.000000,1000.000,0.000000,0.000000]) #协方差信息矩阵，暂定这样的懒得算了
    weight = np.array([1, 0, 0, 1, 0, 1]*1000,dtype=np.float)
    
    with open("DK.g2o","w") as file_DK:
        for i in range(1,len(vertices)):
            theta1 = vertices[i-1,3]
            theta2 = vertices[i,3]
            v1 = vertices[i-1,1:4].copy()
            v2 = vertices[i,1:4].copy()
            v1[-1] = theta1
            v2[-1] = theta2
            A = np.dot(np.linalg.inv(v2t(v1)),v2t(v2))
            trans = t2v(A)
            # trans = v2 - v1
            if i == 1:
                print(trans)
            # idfrom_idto = np.array([vertices[i-1,0],vertices[i,0]])
            idfrom_idto = np.array([i-1,i])
            each = (np.hstack((idfrom_idto,trans,weight)))
            file_DK.write('EDGE_SE2 %d %d %f %f %f %f %f %f %f %f %f\n'%(each[0], each[1], each[2], each[3], each[4],each[5], each[6],each[7], each[8], each[9], each[10]))
    file_DK.close()
    print('EDGE_relative Write Success')

def loopClosureDetect(pos_grid):
    '''
    file    :loop_closure_f.py
    descri  :
    time    :2021/11/30 14:47:44
    author :binyao
    version :1.0
    input   :
            pos_grid # 0 X, 1 Y, 2 Z, 3 ori, 4 magx, 5 magy, 6 magz, 7 mag_xyz
    output  :
    '''
    import matplotlib.pyplot as plt    
    from visualiz_f import visualLoopClosureOnce

    indoor_idx = 0 #室内车库开始的位置
    window_slide = 200 #长度为window_slide/10 m的滑窗
    sim_loop = []

    image_list = []
    fig = plt.figure()
    plt.ion()

    for i in range(window_slide, len(pos_grid) - window_slide, int(window_slide/2)): #直接先留出一个窗口长度作为历史轨迹

        traj_now = pos_grid[i:i + window_slide,:]
        traj_history = pos_grid[:i,:]
        s1_mag = traj_now[:,4:7]
        s2_mag = traj_history[:,4:7]
        s1_pos = traj_now[:,0:3]
        s2_pos = traj_history[:,0:3]
        dist = np.reshape(np.sum(s1_mag**2,axis=1),(s1_mag.shape[0],1))+ np.sum(s2_mag**2,axis=1)-2*s1_mag.dot(s2_mag.T)
        alignmentOBE = dtw.dtw(dist, keep_internals=True, step_pattern=dtw.asymmetric,open_end=True,open_begin=True)
        # # alignmentOBE.plot(type="twoway",offset=0)
        # # print('%d,%d'%(len(alignmentOBE.index1),len(alignmentOBE.index2)))
        # if not constraint(s1_pos[alignmentOBE.index1],s2_pos[alignmentOBE.index2]): #不满足约束条件就continue
        #     continue
        # tmp = [i+alignmentOBE.index1, indoor_idx+alignmentOBE.index2]
        # sim_loop.append(tmp)

        # s1 = pos_grid[i:i + window_slide,:]
        # entropy = SampEn(s1_mag, r= np.std(s1_mag), m = 8)
        # print(entropy)

        # magSequence3DTo1(s1,s1_mag)
        # s2 = pos_grid[:i,:]
        
        # s2_mag = pos_grid_mag3to1[:i*3]
        # print(i,i + window_slide,len(s1), len(s2_mag))
        
        # plt.show()
        # plt.pause(1)
        # plt.clf()
        # s1_mag = s1[:,7]
        # s1_mag = s2[:,7]
        # s1_pos = s1[:,0:3]
        # s2_pos = s2[:,0:3]
        # alignmentOBE = dtw.dtw(s1_mag, s2_mag,keep_internals=True,step_pattern=dtw.asymmetricP1,open_end=True,open_begin=True)
        # #出来的序列投票法选位置
        # alignmentIndexVote(alignmentOBE.index1, alignmentIndexS1)
        # alignmentIndexVote(alignmentOBE.index2, alignmentIndexS2)
        # print(alignmentOBE.index2, alignmentIndexS2)
        # input()
        # alignmentOBE.plot(type="twoway",offset=0)
        # print('%d,%d'%(len(alignmentOBE.index1),len(alignmentOBE.index2)))
        if not constraint(pos_grid[i+alignmentOBE.index1],pos_grid[alignmentOBE.index2]):
            continue
        sim_loop.append([i+alignmentOBE.index1, alignmentOBE.index2])
        # print(sim_loop)
        
        visualLoopClosureOnce(pos_grid, [i+alignmentOBE.index1, alignmentOBE.index2], fig)
        # plt.savefig('temp.png')
        # image_list.append(imageio.imread('temp.png'))
        plt.clf()